﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using static MAVLink;
using System.Threading.Tasks;
using DroneLib.Communication;
using System.ComponentModel;

namespace DroneLib.Vehicles
{
    public partial class FixedWing : Plane
    {
        internal readonly static Dictionary<PLANE_MODE, string> _ModeStrings = new Dictionary<PLANE_MODE, string>()
        {
            {PLANE_MODE.FLY_BY_WIRE_A,"线控A" },
            {PLANE_MODE.AUTO,"自动" },
            {PLANE_MODE.AUTOTUNE,"自动调参" },
            {PLANE_MODE.GUIDED,"引导" },
            {PLANE_MODE.FLY_BY_WIRE_B,"线控B（定高）" },
            {PLANE_MODE.LOITER,"留待" },
            {PLANE_MODE.MANUAL,"手动" },
            {PLANE_MODE.RTL,"返航" },
            {PLANE_MODE.STABILIZE,"增稳" },
            {PLANE_MODE.TAKEOFF,"起飞" },

            {PLANE_MODE.CIRCLE,"盘旋" },
            {PLANE_MODE.CRUISE,"巡航" },

            {PLANE_MODE.ACRO,"特技" },

            {PLANE_MODE.QACRO,"垂起特技" },
            {PLANE_MODE.QAUTOTUNE,"垂起自动调参" },
            {PLANE_MODE.QHOVER,"垂起悬停" },
            {PLANE_MODE.QLAND,"垂起着陆" },
            {PLANE_MODE.QLOITER,"垂起留待" },
            {PLANE_MODE.QRTL,"垂起返航" },
            {PLANE_MODE.QSTABILIZE,"垂起增稳" },

            {PLANE_MODE.THERMAL,"热气流" },
            {PLANE_MODE.TRAINING,"训练" },
            //{PLANE_MODE.AVOID_ADSB,"AVOID ADSB" },
            //{PLANE_MODE.INITIALIZING,"初始化" },
        };

        // 不可设置的模式，相当于只读
        internal readonly static Dictionary<PLANE_MODE, string> _ModeStrings2 = new Dictionary<PLANE_MODE, string>()
        {
            {PLANE_MODE.AVOID_ADSB,"AVOID ADSB" },
            {PLANE_MODE.INITIALIZING,"初始化" },
        };

        public override bool IsRtlMode => _ModeCode == (int)PLANE_MODE.RTL;

        public override bool IsAutoMode => _ModeCode == (int)PLANE_MODE.AUTO;

        public override bool IsAutoTuneMode => _ModeCode == (int)PLANE_MODE.AUTOTUNE;

        public override bool IsManualMode => _ModeCode != (int)PLANE_MODE.AUTO && _ModeCode != (int)PLANE_MODE.RTL && _ModeCode == (int)PLANE_MODE.QRTL;

        internal FixedWing(byte sysId, MavStream link, MAV_TYPE type)
            : base(sysId, link)
        {
            Type = type;
        }        

        public FixedWing()
            :base(0,null)
        {

        }

        public override async Task<bool> GotoLatLng(float lat, float lng, float alt,bool ignoreYaw, MAV_FRAME frame)
        {
            float x, y;
            if (frame == MAV_FRAME.GLOBAL_RELATIVE_ALT_INT)
            {
                x = lat * 1e7f;
                y = lng * 1e7f;
            }
            else
            {
                x = lat;
                y = lng;
            }


            this.SetMode("GUIDED");
            mavlink_mission_item_int_t req = new mavlink_mission_item_int_t
            {
                target_system = SysId,
                target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                command = (UInt16)MAV_CMD.WAYPOINT,
                current = 2, // 此处填 2！！！！！不能填0或者1！！！！
                autocontinue = 1,
                mission_type = (byte)MAV_MISSION_TYPE.MISSION,
                frame = (byte)frame,
                y = (int)y,
                x = (int)x,
                z = (float)alt,
                param1 = 0,
                param2 = 0,
                param3 = 0,
                param4 = 0,
                seq = 0
            };

            var rsp =await WaitResponse(MAVLINK_MSG_ID.MISSION_ITEM_INT, req, 3000, MAVLINK_MSG_ID.MISSION_ACK);
            if(rsp!=null)
            {
                var ack = (mavlink_mission_ack_t)rsp.data;
                var result = (MAV_MISSION_RESULT)ack.type;
                return result == MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED;
            }
            return false;
        }

        //public override async Task<bool> GotoLatLng(float lat, float lng, float alt, bool ignoreYaw, MAV_FRAME frame)
        //{
        //    if (this.ModeCode != (UInt32)COPTER_MODE.GUIDED)
        //    {
        //        bool b = await SetMode((UInt32)COPTER_MODE.GUIDED);
        //        if (b == false)
        //            return false;
        //    }

        //    UInt16 typeMask = UInt16.MaxValue;
        //    ushort MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2));
        //    ushort MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE = ((0 << 0) | (0 << 1) | (1 << 2));
        //    ushort MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5));
        //    ushort MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8));
        //    ushort MAVLINK_SET_POS_TYPE_MASK_FORCE = ((1 << 9));
        //    ushort MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE = ((1 << 10) | (1 << 11));

        //    //typeMask -= MAVLINK_SET_POS_TYPE_MASK_FORCE;

        //    if (lat != 0 && lng != 0)
        //        typeMask -= MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
        //    if (lat == 0 && lng == 0)
        //        typeMask -= MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE;

        //    if (ignoreYaw)
        //        typeMask -= MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;


        //    switch (frame)
        //    {
        //        case MAV_FRAME.GLOBAL_RELATIVE_ALT_INT:
        //            {
        //                mavlink_set_position_target_global_int_t msg = new mavlink_set_position_target_global_int_t()
        //                {
        //                    target_system = this.SysId,
        //                    target_component = (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
        //                    alt = alt,
        //                    lat_int = (int)(lat * 1e7),
        //                    lon_int = (int)(lng * 1e7),
        //                    coordinate_frame = (byte)frame,
        //                    vx = (float)0,
        //                    vy = (float)0,
        //                    vz = (float)0,
        //                    yaw = (float)0,
        //                    yaw_rate = (float)0
        //                };

        //                msg.type_mask = typeMask;

        //                SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, msg);

        //                break;
        //            }
        //        case MAV_FRAME.GLOBAL_INT:
        //            {
        //                mavlink_set_position_target_global_int_t msg = new mavlink_set_position_target_global_int_t()
        //                {
        //                    target_system = this.SysId,
        //                    target_component = (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
        //                    alt = alt,
        //                    lat_int = (int)(lat),
        //                    lon_int = (int)(lng),
        //                    coordinate_frame = (byte)frame,
        //                    vx = (float)0,
        //                    vy = (float)0,
        //                    vz = (float)0,
        //                    yaw = (float)0,
        //                    yaw_rate = (float)0
        //                };

        //                msg.type_mask = typeMask;

        //                SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_GLOBAL_INT, msg);

        //                break;
        //            }
        //        case MAV_FRAME.BODY_FRD:
        //        case MAV_FRAME.BODY_NED:
        //            {
        //                mavlink_set_position_target_local_ned_t msg = new mavlink_set_position_target_local_ned_t()
        //                {
        //                    x = lat,
        //                    y = lng,
        //                    z = 0 - alt,
        //                    vx = 0,
        //                    vy = 0,
        //                    vz = 0,
        //                    afx = 0,
        //                    afy = 0,
        //                    afz = 0,
        //                    yaw = 0,
        //                    yaw_rate = 0,
        //                    type_mask = typeMask,
        //                    target_system = this.SysId,
        //                    target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
        //                    coordinate_frame = (byte)frame
        //                };


        //                SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, msg);

        //                break;
        //            }
        //    }


        //    return true;

        //}

        public override async Task<bool> GotoXY(float x, float y, float z, bool ignoreYaw = false)
        {
            if (this.ModeCode != (UInt32)PLANE_MODE.GUIDED)
            {
                bool b = await SetMode((UInt32)PLANE_MODE.GUIDED);
                if (b == false)
                    return false;
            }

            //UInt16 typeMask;
            //if (ignoreYaw)
            //    typeMask = 0xFFF & (~((1 << 0) | (1 << 1) | (1 << 2) | (1 << 11)));
            //else
            //    typeMask = 0xFFF & (~((1 << 0) | (1 << 1) | (1 << 2)));

            UInt16 typeMask = UInt16.MaxValue;
            ushort MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2));
            ushort MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE = ((0 << 0) | (0 << 1) | (1 << 2));
            ushort MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5));
            ushort MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE = ((1 << 6) | (1 << 7) | (1 << 8));
            ushort MAVLINK_SET_POS_TYPE_MASK_FORCE = ((1 << 9));
            ushort MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE = ((1 << 10) | (1 << 11));

            //typeMask -= MAVLINK_SET_POS_TYPE_MASK_FORCE;

                typeMask -= MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
                typeMask -= MAVLINK_SET_POS_TYPE_MASK_ALT_IGNORE;

            if (ignoreYaw)
                typeMask -= MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;


            mavlink_set_position_target_local_ned_t msg = new mavlink_set_position_target_local_ned_t(
                    0,
                    0,0,0,//x, y, 0 - z,
                    1, 0, 0,
                    0, 0, 0,
                    0, 0,
                    0x0FC7,//typeMask,
                    this.SysId,
                    (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                    (byte)MAV_FRAME.BODY_NED
                );

            SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, msg);
            return true;
        }

        public override async Task<bool> SetYaw(float yaw)
        {
            UInt16 typeMask = 0xFFFF & (~((1 << 3) | (1 << 4) | (1 << 5) | (1 << 10) | (1 << 11)));
            float value = (float)(yaw * Math.PI / 180);
            mavlink_set_position_target_local_ned_t msg = new mavlink_set_position_target_local_ned_t(
                    0,
                    0, 0, 0,
                    0, 0, 0,
                    0, 0, 0,
                    value, value * 2,
                    typeMask,
                    this.SysId,
                    (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1,
                    (byte)MAV_FRAME.BODY_NED
                );

            SendMessage(MAVLINK_MSG_ID.SET_POSITION_TARGET_LOCAL_NED, msg);
            return true;
        }

        protected override uint TranslateModeString(string mode)
        {
            PLANE_MODE planeMode;
            if (Enum.TryParse<PLANE_MODE>(mode, true, out planeMode))
            {
                return (UInt32)planeMode;
            }

            var m = _ModeStrings
                .Where(k => k.Value.Equals(mode))
                .Concat(_ModeStrings2.Where(k => k.Value.Equals(mode)));
            if (m.Count() > 0)
                return (UInt32)m.ElementAt(0).Key;
            else 
                return UInt32.MaxValue;

            foreach (var key in _ModeStrings.Keys)
            {
                if (_ModeStrings[key].Equals(mode))
                    return (uint)key;
            }

            return uint.MaxValue;
        }

        public override string[] GetModeNames()
        {
            return _ModeStrings.Values.ToArray();
        }

        public override string GetModeName(UInt32 modeCode)
        {
            PLANE_MODE mode = (PLANE_MODE)modeCode;
            if (_ModeStrings.ContainsKey(mode))
                return _ModeStrings[mode];
            else if (_ModeStrings2.ContainsKey(mode))
                return _ModeStrings2[mode];
            else
                return mode.ToString();
        }

        public override async Task<bool> SetMode(string modeName)
        {
            UInt32 modeCode = TranslateModeString(modeName);

            if (modeCode == UInt32.MaxValue)
                return false;

            bool b = await base.SetMode(modeCode);
            if (b)
                _ModeCode = modeCode;
            return b;
        }

        //private string GetModeName()
        //{
        //    if (_ModeStrings.ContainsKey((PLANE_MODE)_ModeCode))
        //        return _ModeStrings[(PLANE_MODE)_ModeCode];
        //    else
        //        return "未知模式";
        //}

        public override async Task<bool> ReturnToLaunch()
        {
            return await SetMode((UInt32)PLANE_MODE.RTL);
        }

        public override async Task<bool> Auto()
        {
            //return await SetMode((UInt32)PLANE_MODE.AUTO, true);
            var result =await WaitMavCmdResult(MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1, MAV_CMD.MISSION_START, 3000, 0, 9999);
            return result.Equals(MAV_RESULT.ACCEPTED);
        }

        public override async Task<bool> Land()
        {
            return await SetMode((UInt32)PLANE_MODE.RTL);
        }

        public override async Task<bool> Loiter()
        {
            return await SetMode((UInt32)PLANE_MODE.LOITER);
        }

        public override async Task<bool> AutoTune()
        {
            return await SetMode((UInt32)PLANE_MODE.AUTOTUNE);
        }
    }
}
